#include "LsLidarCh128x1.h"
#include <cmath>
#include <cstring>
#include <iostream>
#include <chrono>
namespace lslidar
{
#define PI ((float)3.1415926)
#define PARAM ((float)0.017453292)    // PARAM
#define ConversionUnit ((float)100.0) // 单位转换 厘米转换成 米
#define ConversionDistance_C_v4_0 \
  ((float)(3.61 / 100.0)) // 光学中心转结构中心的 距离 （3.61 单位为 厘米） 转成
                          // 米为单位
#define ConversionAngle_C_v4_0 \
  ((float)20.25) // 光学中心转结构中心的 角度  单位为 度
#define IDistanceACC_C_v4_0 \
  ((float)(0.4 / 100.0)) // 距离精度转换	 0.4cm	转换成 1m
                         // 雷达传上来的数据1代表 0.4cm
#define DIFOP_PACK_HEADER (0xA5FF005A)
#define POINT_FRAME_START (0xFFAABBCC)
#define POINT_FRAME_START (0xFFAABBCC)

  struct LidarPointXYZ
  {
    float x;
    float y;
    float z;
  };

  static float _sinTheta1[128] = {0}; // 竖直角
  static float _sinTheta2[128] = {0}; // 竖直角
  static float _cosTheta1[128] = {0}; // 竖直角
  static float _cosTheta2[128] = {0}; // 竖直角
  static double BigAngle[32] = {
      -17, -16, -15, -14, -13, -12, -11, -10, -9, -8, -7,
      -6, -5, -4.125, -4, -3.125, -3, -2.125, -2, -1.125, -1, -0.125,
      0, 0.875, 1, 1.875, 2, 3, 4, 5, 6, 7};
  static uint64_t lidarTmConvert(LidarInfo &lidar_info);
  static void
  getOffsetAngle(const std::vector<int> &angle_value); // 获取雷达补偿角
  static uint32_t Combine32bitDataBigEndian(const uint8_t *data_ptr);

  LslidarCh128x1::LslidarCh128x1(){

  };

  LslidarCh128x1::~LslidarCh128x1() {}

  void LslidarCh128x1::start()
  {
    for (int i = 0; i < 128; i++)
    {
      _sinTheta1[i] = sin(BigAngle[i / 4] * PARAM);
      _sinTheta2[i] = sin((i % 4) * (-0.17) * PARAM);
      _cosTheta1[i] = cos(BigAngle[i / 4] * PARAM);
      _cosTheta2[i] = cos((i % 4) * (-0.17) * PARAM);
    }

    _isExit = false;
    _lidarDataDeque.clear();
    this->_decodeThread.reset(
        new std::thread(std::bind(&LslidarCh128x1::decodeLoop, this)));
  }

  void LslidarCh128x1::stop()
  {
    _isExit = true;
    _lidarDataCond.notify_all();
    if ((_decodeThread != nullptr) && _decodeThread->joinable())
    {
      _decodeThread->join();
      _decodeThread = nullptr;
    }
    _lidarDataDeque.clear();
    std::cout << "LslidarCh128x1 Exit ..." << std::endl;
  }

  void LslidarCh128x1::setLidarFrameCallback(
      std::function<void(const uint64_t &tm,
                         const std::vector<LidarPoint> &point_frame)>
          callback)
  {
    _pointFrameCallback = callback;
  }

  void LslidarCh128x1::lidarDataArrived(uint8_t *pdata, uint16_t len)
  {
    std::vector<uint8_t> dataTmp(pdata, pdata + len);
    {
      std::unique_lock<std::mutex> lck{_lidarDataMutex};
      _lidarDataDeque.emplace_front(dataTmp);
    }
    _lidarDataCond.notify_one();
  }

  void LslidarCh128x1::decodeLoop()
  {
    while (!_isExit)
    {
      std::unique_lock<std::mutex> lck(_lidarDataMutex);
      _lidarDataCond.wait(lck,
                          [this]
                          { return _isExit || !_lidarDataDeque.empty(); });
      if (_isExit)
        break;
      auto data = _lidarDataDeque.front();
      _lidarDataDeque.pop_front();
      lck.unlock();
      if (Combine32bitDataBigEndian(data.data()) == DIFOP_PACK_HEADER)
      {
        lidarDifopProccess(data);
        continue;
      }
      else
      {
        lidarMsopProccess(data);
      }
      _lidarInfo.package_tm_last = _lidarInfo.package_tm;
    }
  }

  void LslidarCh128x1::lidarDifopProccess(std::vector<uint8_t> &data)
  {
    _lidarInfo.time_sync_type = (TimeSyncType)(data[44]);
    _lidarInfo.package_year = data[52] + 2000;
    _lidarInfo.package_month = data[53];
    _lidarInfo.package_day = data[54];
    _lidarInfo.package_hour = data[55];
    _lidarInfo.package_minute = data[56];
    _lidarInfo.package_second = data[57];

    std::vector<int> m_anglePrism;
    for (int i = 0; i < 5; i++)
    {
      int16_t angleP =
          (((int16_t)(data[240 + i * 2])) << 8) + (int16_t)data[241 + i * 2];
      m_anglePrism.push_back(angleP);
    }
    getOffsetAngle(m_anglePrism);
  }
  void LslidarCh128x1::lidarMsopProccess(std::vector<uint8_t> &data)
  {
    _lidarInfo.package_tm_nsec =
        (((uint64_t)data[1200]) << 24) + (((uint64_t)data[1201]) << 16) +
        (((uint64_t)data[1202]) << 8) + (((uint64_t)data[1203]) << 0);
    _lidarInfo.echo_type = SINGLE_ECHO_MODEL;
    if (_lidarInfo.time_sync_type == TIME_SYNC_GPS)
    {
      _lidarInfo.package_tm_nsec *= 1000;
    }
    // 单回波
    if (_lidarInfo.echo_type == SINGLE_ECHO_MODEL)
    {
      handleSingleEcho(data);
    }
    // 双回波
    else
    {
      handleDoubleEcho(data);
    }
  }

  void LslidarCh128x1::xyzCalculate(LidarPoint &point)
  {
    double round = _cosTheta2[point.id] * _cosTheta1[point.id] *
                       cos((point.hangle / 2.0) * PARAM) -
                   _sinTheta2[point.id] * _sinTheta1[point.id];
    float sin_Theat = _sinTheta1[point.id] + 2 * round * _sinTheta2[point.id];
    float sin_Theat_2 = 1 - sin_Theat * sin_Theat;
    float cos_Theta = sqrt(sin_Theat_2);
    float data = point.hangle * PARAM;
    float data2 = point.distance * cos_Theta;
    point.x = (data2 * cos(data));
    point.y = (data2 * sin(data));
    point.z = (point.distance * sin_Theat);
  }

  // 单回波处理
  void LslidarCh128x1::handleSingleEcho(std::vector<uint8_t> &data)
  {
    _lidarInfo.package_hour = data[1197];
    _lidarInfo.package_minute = data[1198];
    _lidarInfo.package_second = data[1199];
    _lidarInfo.package_tm =
        lidarTmConvert(_lidarInfo) * 1e+9 + _lidarInfo.package_tm_nsec;
    _lidarInfo.package_point_interal_nsec =
        (_lidarInfo.package_tm - _lidarInfo.package_tm_last) * 1.0 / 171;
    for (int i = 0; i < 1197; i = i + 7)
    {
      /*帧第一个点标志*/
      if (Combine32bitDataBigEndian(data.data() + i) == POINT_FRAME_START)
      {
        _lidarInfo.first_point_flag = true;
      }
      if (_lidarInfo.first_point_flag)
      {
        /* todo callback*/
        if (_pointFrameCallback != nullptr)
        {
          _pointFrameCallback(_lidarInfo.package_tm, _lidarPointFrame);
        }
        _lidarPointFrame.clear();
        _lidarInfo.first_point_flag = false;
        continue;
      }
      else
      {
        if (data[i] < 128)
        {
          LidarPoint m_lidarPoint;
          m_lidarPoint.id = data[i];
          m_lidarPoint.hangle =
              (((int)(data[i + 1]) << 8) + (int)data[i + 2]) / 100.f;
          m_lidarPoint.distance = (((int)(data[i + 3]) << 16) +
                                   ((int)(data[i + 4]) << 8) + (int)data[i + 5]) /
                                  256.0 / ConversionUnit;
          m_lidarPoint.intensity = data[i + 6];
          if (m_lidarPoint.distance <= 0)
            continue;
          xyzCalculate(m_lidarPoint);
          _lidarPointFrame.push_back(m_lidarPoint);
        }
      }
    }
  }
  // 双回波处理
  void LslidarCh128x1::handleDoubleEcho(std::vector<uint8_t> &data)
  {
    _lidarInfo.package_tm =
        lidarTmConvert(_lidarInfo) * 1e+9 + _lidarInfo.package_tm_nsec;
    _lidarInfo.package_point_interal_nsec =
        (_lidarInfo.package_tm - _lidarInfo.package_tm_last) * 1.0 / 109;

    for (int i = 0; i < 1199; i = i + 11)
    {
      /*帧第一个点标志*/
      if (Combine32bitDataBigEndian(&data[i]) == POINT_FRAME_START)
      {
        _lidarInfo.first_point_flag = true;
      }
      if (_lidarInfo.first_point_flag)
      {
        if (_pointFrameCallback != nullptr)
        {
          _pointFrameCallback(_lidarInfo.package_tm, _lidarPointFrame);
        }
        _lidarInfo.first_point_flag = false;
        _lidarPointFrame.clear();
        continue;
      }
      else
      {
        if (data[i] < 128)
        {
          LidarPoint m_lidarPoint;
          m_lidarPoint.id = data[i];
          m_lidarPoint.hangle =
              (((int)(data[i + 1]) << 8) + (int)data[i + 2]) / 100.f;
          m_lidarPoint.distance = (((int)(data[i + 3]) << 16) +
                                   ((int)(data[i + 4]) << 8) + (int)data[i + 5]) /
                                  256.0 / ConversionUnit;
          m_lidarPoint.intensity = data[i + 6];
          if (m_lidarPoint.distance > 0)
          {
            xyzCalculate(m_lidarPoint);
            _lidarPointFrame.push_back(m_lidarPoint);
          }

          m_lidarPoint.distance = (((int)(data[i + 7]) << 16) +
                                   ((int)(data[i + 8]) << 8) + (int)data[i + 9]) /
                                  256.0 / ConversionUnit;
          m_lidarPoint.intensity = data[i + 10];
          if (m_lidarPoint.distance > 0)
          {
            xyzCalculate(m_lidarPoint);
            _lidarPointFrame.push_back(m_lidarPoint);
          }
        }
      }
    }
  }

  // 获取雷达补偿角
  static void getOffsetAngle(const std::vector<int> &angle_value)
  {
    double m_prismAngle[4];
    float m_prismOffsetVAngle = 0.0f;

    if (abs(angle_value[0]) != 0)
    {
      m_prismOffsetVAngle = angle_value[0] / 100.0;

      for (int i = 0; i < 128; i++)
      {
        if (i / 4 % 2 == 0)
        {
          _sinTheta1[i] = sin((BigAngle[i / 4] + m_prismOffsetVAngle) * PARAM);
          _cosTheta1[i] = cos((BigAngle[i / 4] + m_prismOffsetVAngle) * PARAM);
        }
        else
        {
          _sinTheta1[i] = sin(BigAngle[i / 4] * PARAM);
          _cosTheta1[i] = cos(BigAngle[i / 4] * PARAM);
        }
      }
    }
    else
    {
      for (int i = 0; i < 128; i++)
      {
        _sinTheta1[i] = sin((BigAngle[i / 4]) * PARAM);
        _cosTheta1[i] = cos((BigAngle[i / 4]) * PARAM);
      }
    }

    if (abs(angle_value[1]) == 0 && abs(angle_value[2]) == 0 &&
        abs(angle_value[3]) == 0 && abs(angle_value[4]) == 0)
    {
      for (int i = 0; i < 4; i++)
      {
        m_prismAngle[i] = i * -0.17;
      }

      for (int i = 0; i < 128; i++)
      {
        _sinTheta2[i] = sin((m_prismAngle[i % 4]) * PARAM);
        _cosTheta2[i] = cos((m_prismAngle[i % 4]) * PARAM);
      }
    }
    else
    {
      for (int i = 0; i < 4; i++)
      {
        m_prismAngle[i] = angle_value[i + 1] / 100.0;
      }
      for (int i = 0; i < 128; i++)
      {
        _sinTheta2[i] = sin((m_prismAngle[i % 4]) * PARAM);
        _cosTheta2[i] = cos((m_prismAngle[i % 4]) * PARAM);
      }
    }
    return;
  }

  static uint32_t Combine32bitDataBigEndian(const uint8_t *data_ptr)
  {
    uint64_t data = 0;
    for (int i = 0; i < 4; i++)
    {
      data = (data << 8) + data_ptr[i];
    }
    return data;
  }

  static uint64_t lidarTmConvert(LidarInfo &lidar_info)
  {
    struct tm m_tm;
    m_tm.tm_sec = lidar_info.package_second;
    m_tm.tm_min = lidar_info.package_minute;
    m_tm.tm_hour = lidar_info.package_hour;
    m_tm.tm_mday = lidar_info.package_day;
    m_tm.tm_mon = lidar_info.package_month - 1;
    m_tm.tm_year = lidar_info.package_year - 1900;
    m_tm.tm_isdst = 0;
    time_t t = static_cast<uint64_t>(timegm(&m_tm));
    return (uint64_t)t;
  }
} // namespace lslidar
